#include <joyos.h>
#include "navigate5.h"
#include <Math.h>
#include <lib/pid.h>

//Measurements
#define WHEEL_DIAMETER (3.5)
#define ROBOT_WIDTH (10.)
#define VPS_PER_FOOT (443.4)
#define VPS_PER_THETA (11.3722)
#define ROBOT_HEIGHT (8.0)
#define CEILING_HEIGHT (129.0)
//IO ports
#define LEFT_MOTOR (1)
#define RIGHT_MOTOR (0)
#define GEAR_MOTOR (2)
//Speeds
#define MIN_SPEED (100)
#define DRIVE_VELOCITY (200)
#define TURN_VELOCITY (150)
//Error ranges
#define MIN_DEGREES (3)
#define DEG_ERROR_RANGE (3)
#define MIN_DEGREES_CLOSE (20)


//states of the robot
uint8_t front = 1;
uint8_t Turning = 0;
uint8_t Driving = 0;
uint8_t DrivingDone = 1;
uint8_t closeToGoal = 0;
//motor velocities
int16_t leftVel = 0;
int16_t rightVel = 0;
//robot values
float myHeading = 0;
Point me = {0, 0};
float initialPos = 0;
uint8_t myColor = 0;
//opponent values
Point opponent = {0, 0};
//limiting error correction
int16_t timeOutTries = 0;
//for the goal Point
Point originalGoal = {0, 0};
Point currentGoal = {0, 0};
uint16_t goalDistance = 2.0/12.;
int16_t goalDegrees = 0;


//Point coordinates in feet ie, me.x = 2.3 is 2ft and 3/10 of a foot.

void switchFrontDir(){
	if(front){
		front = 0;
	}else{
		front = 1;
	}
}

uint8_t getFront(){
	return front;
}

void getData(){
	copy_objects();
	float rawX = game.coords[0].x/VPS_PER_FOOT*12.0;
	float rawY = game.coords[0].y/VPS_PER_FOOT*12.0;
	float rawR = sqrt(rawX*rawX + rawY*rawY);								
	float theta = atan2(rawY,rawX);
	float primeR = rawR*ROBOT_HEIGHT/CEILING_HEIGHT;
	float myR = rawR - primeR;
	me.x = myR*cos(theta)/12.0;
	me.y = myR*sin(theta)/12.0; 


	rawX = game.coords[1].x/VPS_PER_FOOT*12.0;
	rawY = game.coords[1].y/VPS_PER_FOOT*12.0;
	rawR = sqrt(rawX*rawX + rawY*rawY);								
	theta = atan2(rawY,rawX);
	primeR = rawR*ROBOT_HEIGHT/CEILING_HEIGHT;
	myR = rawR - primeR;
	opponent.x = myR*cos(theta)/12.0;
	opponent.y = myR*sin(theta)/12.0; 

	//printf("\nMyY: %f", me.y);
	myHeading =  (game.coords[0].theta/VPS_PER_THETA);
	//printf("\nf: %d", myHeading);
	
}


Point getMyPosition(){
	getData();
	return me;
}

Point getOpponentPosition(){
	getData();
	return opponent;
}

int getTerritoryOwner(uint8_t territory){
	getData();
	return game.territories[territory - 1].owner;
}

int getTerritorySheepLeft(uint8_t territory){
	getData();
	return game.territories[territory - 1].remaining;
}

int getTerritorySheepTimeLeft(uint8_t territory){
	getData();
	return game.territories[territory - 1].rate_limit;
}
float getMyHeading(){
	getData();
	return myHeading;
}

Point getCurrentGoal(){
	return currentGoal;
}

void setMyColor(uint8_t color){//blue is 1, red is 0
	myColor = color;
}

uint8_t getMyColor(){
	return myColor;//blue is 1, red is 0
}

void setNextGoal(Point goal){
	currentGoal.x = goal.x;
	currentGoal.y = goal.y;
}

int16_t limitVel(float vel){
  if (vel < -255) return -255;
  if (vel > 255) return 255;
  return (int16_t)vel;
}

int16_t calculateAngle(Point goal){
	getData();
	
	float deltax = goal.x- me.x;
	float deltay = goal.y - me.y;
	float degreescalc = atan2(deltay, deltax)*360/2/3.14159625;
	int16_t degrees = (int16_t) degreescalc;

	if(front){
		degrees = myHeading - degrees + 360;
	}else{
		degrees = myHeading - degrees + 180;
	}
	
	if (degrees < 0){
		degrees = degrees + 360;
	}
	degrees = degrees % 360;

	return degrees;
}

/*int16_t calculateAngle2(Point goal){
	getData();
	
	float deltax = goal.x- me.x;
	float deltay = goal.y - me.y;
	float degreescalc = atan2(deltay, deltax)*360/2/3.14159625;
	int16_t degrees = (int16_t) degreescalc;
	
	if (degrees < 0){
		degrees = degrees + 360;
	}
	
	degrees = degrees % 360;
	
	return degrees;
}*/

float calculateDistance(Point goal, Point start){
	float deltax = goal.x- start.x;
	float deltay = goal.y - start.y;
	return sqrt(deltax*deltax + deltay*deltay);
}

Point calculateNextGoal(float distance){
	
	printf("calculating anotherGoal");
	int16_t direction = (int16_t)(getMyHeading() + 360) % 360;
	Point newGoal = getMyPosition();
	
	newGoal.x = newGoal.x + cos(direction) * distance;
	newGoal.y = newGoal.y + sin(direction) * distance;

	return newGoal;
}
void setVelocities(float correctionAmount){								
  motor_set_vel(LEFT_MOTOR, limitVel((float)leftVel + correctionAmount));
  motor_set_vel(RIGHT_MOTOR, limitVel((float)rightVel + correctionAmount));
}

/*float correctedGyroscope(){
	int16_t degreeOff = calculateAngle(currentGoal);
	
	if (degreeOff > 180){
		degreeOff = degreeOff - 360;
	}
	
	if (fabs(degreeOff) > MIN_DEGREES){
		return -degreeOff;
	}else{
		return 0;
	}
}
float correctedGyroscope2(){
	float gyro = gyro_get_degrees();
	return initialPos - gyro;
}
*/

void speedUp(){
	if(front){
		leftVel++;
		rightVel--;
	}else{
		leftVel--;
		rightVel++;
	}
}

void slowDown(){
	if(front){
		leftVel--;
		rightVel++;
	}else{
		leftVel++;
		rightVel--;
	}
}

void setSpeed(uint16_t speed){
	if (front){
		leftVel = speed;
		rightVel = -speed;
	}else{
		leftVel = -speed;
		rightVel = speed;
	}
}

void turn(uint16_t goalTurnDegrees){
	
	//printf("\nturnx degrres %u", goalTurnDegrees);
	float turnedDeg = fabs(initialPos - gyro_get_degrees());

	if(turnedDeg > goalTurnDegrees){
			Turning = 0;
			leftVel = 0;
			rightVel = 0;
	}
		
	motor_set_vel(RIGHT_MOTOR, rightVel);
	motor_set_vel(LEFT_MOTOR, leftVel);
}



void turnDegrees(float deg, uint16_t speed ){

	uint16_t turnDeg = deg;
	uint32_t start_time = get_time();
	
	if (deg <= 90){
		leftVel = speed;
		rightVel = speed;
	}else if (deg < 180){
		turnDeg = 180 - deg;
		switchFrontDir();
		leftVel = -speed;
		rightVel = -speed;
	}else if( deg == 180){
		switchFrontDir();
		turnDeg = 0;
		leftVel = -speed;
		rightVel = -speed;
	}else if (deg <= 270){
		turnDeg = deg - 180;
		switchFrontDir();
		leftVel = speed;
		rightVel = speed;
	}else if (deg == 360 ){
		turnDeg = 0;
	}else{
		turnDeg = 360 - deg;	
		leftVel = -speed;
		rightVel = -speed;
	}	

	Turning = 1;
	initialPos = gyro_get_degrees();
	
	if(turnDeg == 0){
		Turning = 0;
	}
	
	while(Turning){
		if (get_time() - start_time > 2000){
			Turning = 0;
		}
		turn(turnDeg);
	}
	
	pause(100);
}

void turnToHeading(int16_t goalDegrees){ //goal heading input as 0 -360
	uint32_t start_time = get_time();
	uint8_t timeOutCount = 0;
	
	pause(100);
	int16_t myPosition = ((int16_t)getMyHeading() + 360) % 360;	
	int16_t turningDegrees = (int16_t)(myPosition - goalDegrees + 360) % 360;
	
	front = 1;
	
	if (turningDegrees > 180){
		leftVel = -TURN_VELOCITY;
		rightVel = -TURN_VELOCITY;
		turningDegrees = 360 - turningDegrees;
	}else{
		leftVel = TURN_VELOCITY;
		rightVel = TURN_VELOCITY;
	}
	
	initialPos = gyro_get_degrees();
	Turning = 1;

	while (fabs(initialPos - gyro_get_degrees()) < turningDegrees - 5 && Turning) {
		motor_set_vel(RIGHT_MOTOR, rightVel);
		motor_set_vel(LEFT_MOTOR, leftVel);
		
		if (get_time() - start_time > 3000){
			Turning = 0;
			driveForwardFeet(6./12., -100);
		}
	}

	motor_set_vel(RIGHT_MOTOR, 0);
	motor_set_vel(LEFT_MOTOR, 0);
	pause(100);
	
}


void driveFeet(float feet, uint16_t speed, float accuracy){
	
	uint32_t start_time = get_time();
	uint16_t distanceToGoal = calculateDistance(me, currentGoal);
	uint16_t distanceCheckTimeout = calculateDistance(me, currentGoal);
	
	setSpeed(speed); 
	
	Driving = 1;
	
	float kP = 1.9; //proportional gain
	float kI = 0; //integral gain
	float kD = 0; //derivative gain

	struct pid_controller headingPID;

	init_pid(&headingPID,
            kP,
            kI,
            kD,
            &correctedGyroscope,
            &setVelocities);

	initialPos = gyro_get_degrees();
	headingPID.goal = 0;
	headingPID.enabled = true;
	
	while(Driving){
		getData();
		distanceToGoal = calculateDistance(me, currentGoal);
		//printf("in driving loop \n");
		
		if(distanceToGoal < accuracy){
			Driving = 0;
			DrivingDone = 1;
			setSpeed(0);
			closeToGoal = 0;
		}else if (distanceToGoal < accuracy + 2./12.){
			
			while(fabs(leftVel) > 75){
				slowDown();
				pause(5);
			}
			setSpeed(75);
		}
		
		
		if (get_time() - start_time > 4000){
			if (distanceCheckTimeout - distanceToGoal < 1./12.){
				Driving = 0;	
				setSpeed(0); 
				motor_set_vel(RIGHT_MOTOR, rightVel);
				motor_set_vel(LEFT_MOTOR, leftVel);
				driveForwardFeet(6./12., -100);
				for (uint8_t i = 0; i< 2; i++){
					turnDegrees(90, 200);
				}
				
				driveForwardFeet(4./12., 100);
				setSpeed(speed);
			}
			distanceCheckTimeout = distanceToGoal;
			start_time = get_time();
		}
		
		update_pid(&headingPID);

	}
	
	setSpeed(0); 
	motor_set_vel(RIGHT_MOTOR, rightVel);
	motor_set_vel(LEFT_MOTOR, leftVel);
	timeOutTries = 0;
	pause(50);
}


void driveForwardFeet(float feet, int16_t speed){	

	Point origin = getMyPosition();
	float distanceDriven = calculateDistance(getMyPosition(), origin);
	uint32_t start_time = get_time();
	
	uint16_t distanceCheckTimeout = 0;
	
	uint8_t timeOut = 0;		
	uint8_t canDrive= 1;
	float distance = feet;
	uint16_t timeOutTime = 1500;
	
	if (speed > 0){
		front = 1;
		setSpeed((uint16_t)speed);
	}else{
		front = 0;
		setSpeed((uint16_t)fabs(speed));
	}
	
	if (feet < 0){
		distance = 1./12.;
		timeOutTime = 1000;
	}else if (feet < 2./12.){
		timeOutTime = 1000;
	}else if (feet > 6./12.){
		timeOutTime = 2000;
	}
	
	
	
	motor_set_vel(RIGHT_MOTOR, rightVel);
	motor_set_vel(LEFT_MOTOR, leftVel);
	
	float kP = 1.8; //proportional gain
	float kI = 0; //integral gain
	float kD = 0; //derivative gain

	struct pid_controller headingPID;

	init_pid(&headingPID,
            kP,
            kI,
            kD,
            &correctedGyroscope2,
            &setVelocities);

	initialPos = gyro_get_degrees();
	headingPID.goal = 0;
	headingPID.enabled = true;
	
	while(canDrive && !timeOut){
		getData();
		distanceDriven = calculateDistance(getMyPosition(), origin);
		
		if(distanceDriven > distance - .5/12.){
			canDrive = 0;
			setSpeed(0);
		}
		
		if (get_time() - start_time > timeOutTime){
			timeOut = 1;
			start_time = get_time();
		}
		
		update_pid(&headingPID);
	}
	
	setSpeed(0);
	motor_set_vel(RIGHT_MOTOR, rightVel);
	motor_set_vel(LEFT_MOTOR, leftVel);
}


void turnToPoint(Point goal){
	int16_t degrees = calculateAngle2(goal);	
	turnToHeading(degrees);
}

void driveToPoint(Point goal, float accuracy){
	
	servo_set_pos(1, 80);
	
	setNextGoal(goal);
	int16_t degrees = calculateAngle(goal);
	float distance = calculateDistance(goal, getMyPosition());
	
	turnDegrees(degrees, TURN_VELOCITY);
	
	driveFeet(distance, DRIVE_VELOCITY, accuracy);		 
}
